/*
 * Copyright (c) 2020 Fraunhofer FOKUS and others. All rights reserved.
 *
 * See the NOTICE file(s) distributed with this work for additional
 * information regarding copyright ownership.
 *
 * This program and the accompanying materials are made available under the
 * terms of the Eclipse Public License 2.0 which is available at
 * http://www.eclipse.org/legal/epl-2.0
 *
 * SPDX-License-Identifier: EPL-2.0
 *
 * Contact: mosaic@fokus.fraunhofer.de
 */

package org.eclipse.mosaic.starter;

import com.google.common.collect.Lists;
import libpython_clj2.java_api;
import org.eclipse.mosaic.fed.carla.clojure.ClojureCarlaConnection;
import org.eclipse.mosaic.fed.carla.clojure.commands.SimulationSimulateStep;
import org.eclipse.mosaic.fed.carla.clojure.commands.VehicleAdd;
import org.eclipse.mosaic.fed.carla.clojure.commands.VehicleRemove;
import org.eclipse.mosaic.fed.carla.clojure.commands.VehicleSynchronize;
import org.eclipse.mosaic.fed.carla.config.CarlaConfiguration;
import org.eclipse.mosaic.lib.enums.DriveDirection;
import org.eclipse.mosaic.lib.geo.CartesianPoint;
import org.eclipse.mosaic.lib.geo.MutableCartesianPoint;
import org.eclipse.mosaic.lib.geo.MutableGeoPoint;
import org.eclipse.mosaic.lib.objects.vehicle.VehicleData;
import org.eclipse.mosaic.lib.objects.vehicle.VehicleSignals;
import org.eclipse.mosaic.lib.objects.vehicle.VehicleTypeData;
import org.eclipse.mosaic.lib.util.JsonUtils;
import org.junit.Ignore;
import org.junit.Test;
import org.junit.runner.RunWith;
import org.mockito.junit.MockitoJUnitRunner;

import java.io.File;
import java.nio.file.Files;
import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.List;
import java.util.Map;
import java.util.stream.Collectors;

@RunWith(MockitoJUnitRunner.class)
public class LibPythonTest {

    @Ignore
    @Test
    public void firstSample() {
        // Find and load python from path
        java_api.initialize(null);

        Object numpy = java_api.importModule("numpy");
        Object ones = java_api.getAttr(numpy, "ones");
        // Prepare parameter
        ArrayList<Integer> param = new ArrayList<>();
        param.add(3);

        Object result = java_api.call(ones, param);
        // method should do call, but the PY object do not
        Object firstItem = java_api.getItem(result, 0);
        double value = (double) java_api.copyToJVM(firstItem);

        System.out.println(value);
    }

    @Ignore
    @Test
    public void testConnection() {
        // initialize python library from source root
        java_api.initialize(null);
        String carlaEggPath = "/home/lenovo/pan2/xie/wict-sim/co-simulation/bridge/PythonAPI/carla/dist/";
        int retryConnectionTimes = 5;
        Object carlaClient;
        Object carlaWorld = null;

        try (AutoCloseable locker = java_api.GILLocker()) {
            // load carla egg

            String file = "import glob\n" +
                    "import os\n" +
                    "import sys\n" +
                    "import time\n" +
                    "\n" +
                    "try:\n" +
                    "    sys.path.append(\n" +
                    "        glob.glob('" + carlaEggPath + "carla-*%d.%d-%s.egg' %\n" +
                    "                  (sys.version_info.major, sys.version_info.minor,\n" +
                    "                   'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])\n" +
                    "except IndexError:\n" +
                    "    print(\"Can not find carla library .egg\")";
            java_api.runStringAsFile(file);

            Object carlaModule = java_api.importModule("carla");
            Object clientAttr = java_api.getAttr(carlaModule, "Client");
            while (retryConnectionTimes > 0) {
                retryConnectionTimes--;
                boolean connected = true;

                try {
                    carlaClient = java_api.call(clientAttr, "localhost", 2000);
                    Object setTimeoutAttr = java_api.getAttr(carlaClient, "set_timeout");
                    // set carla client timeout 5s
                    java_api.call(setTimeoutAttr, 5);
                    Object getWorldAttr = java_api.getAttr(carlaClient, "get_world");
                    carlaWorld = java_api.call(getWorldAttr);
                } catch (Exception e) {
                    connected = false;
                }

                if (connected) {
                    break;
                }
                if (retryConnectionTimes == 0) {
                    throw new RuntimeException("Can not connect CARLA server. Check whether CARLA server starts or not.");
                }
            }

            Object getBlueprintLibraryAttr = java_api.getAttr(carlaWorld, "get_blueprint_library");
            Object carlaBlueprintLibrary = java_api.call(getBlueprintLibraryAttr);

            // Configuring carla simulation in sync mode
            Object getSettingsAttr = java_api.getAttr(carlaWorld, "get_settings");
            Object settings = java_api.call(getSettingsAttr);
            java_api.setAttr(settings, "synchronous_mode", Boolean.TRUE);
            java_api.setAttr(settings, "fixed_delta_seconds", 0.1);
            Object applySettingsAttr = java_api.getAttr(carlaWorld, "apply_settings");
            // java_api.call(applySettingsAttr, settings);

            // runAsInput
            java_api.setGlobal("blueprint_library", carlaBlueprintLibrary);
            Object bpIdsPy = java_api.runStringAsInput("[bp.id for bp in blueprint_library]");
            List<String> bpIds = (List<String>) java_api.copyToJVM(bpIdsPy);
            System.out.println(bpIds.stream().collect(Collectors.joining(",")));

            // read json
            File vTypesFile = new File("/home/lenovo/pan2/xie/wict-sim/co-simulation/bridge/carla_integration/vtypes.json");
            String vTypesString = new String(Files.readAllBytes(Paths.get(vTypesFile.getPath())));
            Map vTypes = JsonUtils.deserialize(vTypesString, Map.class);
            vTypes = (Map) vTypes.get("carla_blueprints");

            java_api.setGlobal("_VTYPES", vTypes);

            Map global = java_api.runStringAsFile(
                    "import random \n" +
                            "import logging \n" +
                            "def _get_recommended_carla_blueprint(v_class):\n" +
                            "        blueprints = []\n" +
                            "        for blueprint in blueprint_library:\n" +
                            "            if blueprint.id in _VTYPES and \\\n" +
                            "               _VTYPES[blueprint.id]['vClass'] == v_class:\n" +
                            "               blueprints.append(blueprint)\n" +
                            "\n" +
                            "        if not blueprints:\n" +
                            "            return None\n" +
                            "\n" +
                            "        return random.choice(blueprints) \n" +
                            "def get_carla_blueprint(type_id, v_class, sync_color=False):\n" +
                            "\n" +
                            "        if type_id in [bp.id for bp in blueprint_library]:\n" +
                            "            blueprint = blueprint_library.filter(type_id)[0]\n" +
                            "        else:\n" +
                            "            blueprint = _get_recommended_carla_blueprint(v_class)\n" +
                            "            if blueprint is not None:\n" +
                            "                logging.warning(\n" +
                            "                    'sumo vtype %s not found in carla. The following blueprint will be used: %s',\n" +
                            "                    type_id, blueprint.id)\n" +
                            "            else:\n" +
                            "                logging.error('sumo vtype %s not supported. No vehicle will be spawned in carla',\n" +
                            "                              type_id)\n" +
                            "                return None\n" +
                            "\n" +
                            "        if blueprint.has_attribute('color'):\n" +
                            "            if sync_color:\n" +
                            "                color = \"{},{},{}\".format(sumo_actor.color[0], sumo_actor.color[1],\n" +
                            "                                          sumo_actor.color[2])\n" +
                            "            else:\n" +
                            "                color = random.choice(blueprint.get_attribute('color').recommended_values)\n" +
                            "            blueprint.set_attribute('color', color)\n" +
                            "\n" +
                            "        if blueprint.has_attribute('driver_id'):\n" +
                            "            driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values)\n" +
                            "            blueprint.set_attribute('driver_id', driver_id)\n" +
                            "\n" +
                            "        blueprint.set_attribute('role_name', 'sumo_driver')\n" +
                            "\n" +
                            "        return blueprint"
            );

            Object getCarlaBlueprint = global.get("get_carla_blueprint");
            Object bluePrint = java_api.call(getCarlaBlueprint, "", "passenger");
            System.out.println(java_api.copyToJVM(bluePrint));


            // prepare sumo transform, extend
            java_api.setGlobal("offset", Lists.newArrayList(681.36,415.46));
            CartesianPoint position = new MutableCartesianPoint(1147.0, 875.1, 0.0);
            double slope = 11.2;
            double angel = 180.0;
            double length = 12.0;
            double height = 2.0;
            double width = 3.0;

            Object locationAttr = java_api.getAttr(carlaModule, "Location");
            Object rotationAttr = java_api.getAttr(carlaModule, "Rotation");
            Object transformAttr = java_api.getAttr(carlaModule, "Transform");
            Object vector3DAttr = java_api.getAttr(carlaModule,  "Vector3D");

            Object location = java_api.call(locationAttr, position.getX(), position.getY(), position.getZ());
            Object rotation = java_api.call(rotationAttr, slope, angel, 0.0);
            Object transform = java_api.call(transformAttr, location, rotation);
            Object extend = java_api.call(vector3DAttr, length / 2.0, width / 2.0, height / 2.0);

            // transform
            Map global2 = java_api.runStringAsFile(
                    "\n" +
                            "import carla \n" +
                            "import math \n" +
                            "def get_carla_transform(in_sumo_transform, extent):\n" +
                            "        in_location = in_sumo_transform.location\n" +
                            "        in_rotation = in_sumo_transform.rotation\n" +
                            "\n" +
                            "        yaw = -1 * in_rotation.yaw + 90\n" +
                            "        pitch = in_rotation.pitch\n" +
                            "        out_location = (in_location.x - math.cos(math.radians(yaw)) * extent.x,\n" +
                            "                        in_location.y - math.sin(math.radians(yaw)) * extent.x,\n" +
                            "                        in_location.z - math.sin(math.radians(pitch)) * extent.x)\n" +
                            "        out_rotation = (in_rotation.pitch, in_rotation.yaw, in_rotation.roll)\n" +
                            "\n" +
                            "        out_location = (out_location[0] - offset[0], out_location[1] - offset[1], out_location[2])\n" +
                            "\n" +
                            "        out_transform = carla.Transform(\n" +
                            "            carla.Location(out_location[0], -out_location[1], out_location[2]),\n" +
                            "            carla.Rotation(out_rotation[0], out_rotation[1] - 90, out_rotation[2]))\n" +
                            "\n" +
                            "        return out_transform"
            );
            Object getCarlaTransform = global2.get("get_carla_transform");
            Object carlaTransform = java_api.call(getCarlaTransform, transform, extend);
            System.out.println(java_api.copyToJVM(carlaTransform));

            // set
            Map global3 = java_api.runStringAsFile(
                    "import numpy as np \n" +
                    "def test():\n"
                            + " return list(set([1,2,3]))");
            Object test = global3.get("test");
            Object set = java_api.call(test);
            java_api.getItem(set, 0);
        } catch (Exception e) {
            throw new RuntimeException(e);
        }
    }


    @Ignore
    @Test
    public void testVehicle() {
        VehicleData sumoVehicleData = new VehicleData.Builder(200000000, "sumo_0")
                .position(new MutableGeoPoint(29.51824813139327,106.33003760351541, 0), new MutableCartesianPoint(1145.703627476927, 474.26481417038093, 0))
                .movement(10.0,1.0, 2.0)
                .signals(new VehicleSignals(false, false, false, false, false, 0))
                .orientation(DriveDirection.UNAVAILABLE, 265.13854781970724, 0)
                .vehicleType(new VehicleTypeData("vehicle.toyota.prius", 1.5373616218566895, 4.538333415985107, 2.0011305809020996))
                .vehicleClass("evehicle")
                .create();


        // initialize python library from source root
        java_api.initialize(null);
        CarlaConfiguration carlaConfiguration = new CarlaConfiguration();
        carlaConfiguration.carlaEggPath = "/home/lenovo/pan2/xie/wict-sim/co-simulation/bridge/PythonAPI/carla/dist/";
        carlaConfiguration.carlaConnectionPort = 2000;
        carlaConfiguration.updateInterval = 100L;
        carlaConfiguration.offset = "681.36,415.46";
        carlaConfiguration.vTypesPath = "/home/lenovo/pan2/xie/wict-sim/co-simulation/bridge/carla_integration/";
        carlaConfiguration.syncVehicleLights = false;
        ClojureCarlaConnection connection = new ClojureCarlaConnection("localhost", 2000, null, carlaConfiguration, 5);
        connection.connect();
        try (AutoCloseable locker = java_api.GILLocker()) {
            SimulationSimulateStep simulateStep = new SimulationSimulateStep();
            simulateStep.execute(connection);

            VehicleAdd vehicleAdd = new VehicleAdd();
            vehicleAdd.execute(connection, sumoVehicleData);

            VehicleSynchronize vehicleSynchronize = new VehicleSynchronize();
            vehicleSynchronize.execute(connection, sumoVehicleData);

            int time = 100;
            while (time > 0) {
                time--;
                Thread.sleep(100);
                simulateStep.execute(connection);
            }

            VehicleRemove vehicleRemove = new VehicleRemove();
            vehicleRemove.execute(connection, "sumo_0");
        } catch (Throwable e) {
            throw new RuntimeException(e);
        } finally {
            connection.close();
        }
    }
}
